from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Point, Quaternion
from visualization_msgs.msg import Marker, MarkerArray

def get_pose_msg(position, orientation):
    '''
    args:
        position: list or array, [x,y,z]
        orientation: list or array, [x,y,z,w]
    return:
        pose_msg: Pose
    '''
    pose_msg = Pose()
    pose_msg.position = Point(x=position[0], y=position[1], z=position[2])
    pose_msg.orientation = Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])
    
    return pose_msg

def get_posestamped_msg(position, orientation, frame_name):
    '''
    args:
        position: list or array, [x,y,z]
        orientation: list or array, [x,y,z,w]
        frame_name: str
    return:
        pose_msg: PoseStamped
    '''
    pose_msg = PoseStamped()
    pose_msg.header.frame_id = frame_name
    pose_msg.pose.position = Point(x=position[0], y=position[1], z=position[2])
    pose_msg.pose.orientation = Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])
    
    return pose_msg

def get_posearray_msg(pose_list, frame_name):
    '''
    args:
        pose_list: list of Pose 
    return:
        pose_msg: PoseArray
    '''
    pose_msg = PoseArray()
    pose_msg.header.frame_id = frame_name
    pose_msg.poses = pose_list
    
    return pose_msg

def get_marker_msg(position, orientation, scale, id, mesh_resource, frame_name, color):
    '''
    args:
        position: list or array, [x,y,z]
        orientation: list or array, [x,y,z,w]
        frame_name: str
        scale: float or list/array
        id: used in Marker
        mesh_resource: str
        color: list or array, [r,g,b,a]
    return:
        marker: Marker
    '''
    marker = Marker()
    marker.header.frame_id = frame_name
    marker.pose.position = Point(x=position[0], y=position[1], z=position[2])
    marker.pose.orientation = Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])
    marker.type = Marker.MESH_RESOURCE
    marker.id = id
    marker.action = Marker.MODIFY
    marker.mesh_resource = mesh_resource
    if isinstance(scale, float):
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale
    else:
        marker.scale.x = scale[0]
        marker.scale.y = scale[1]
        marker.scale.z = scale[2]
    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = color[3]

    return marker


def get_markerarray_msg(marker_list):
    marker_array = MarkerArray()
    marker_array.markers = marker_list
    return marker_array
